#define _MAIN_CPP_

#include <iostream>
#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>
#include "smat-ransac.h"


#include "videoAVI.h"
#include "constants.h"
#include "face_detect_vj.hh"
#include "modelo_RANSAC.h"
#include "read_video_thread.h"
#include "other_functions.h"
#include "options.h"
#include <colores.h>

#include "facedir.h"

//#define USE_THREAD_RANSAC

char window[2][100] = { "Camara Left", "Camara Right"};
char Model_SMAT[2][100] = { "Model+SMAT Left", "Model+SMAT Right"};


int bucle_cvWaitKey(int * wait);


int main(int argc, char *argv[])
{
    if (!leer_opciones(argc, argv))
	return 0;

    //srand( (unsigned)time( NULL ) );
    srand(1);  //siempre la misma secuencia aleatoria. Debug

    CFacedir facedir;

    facedir.graficar = ini_opt("graficar");
    /***********/
    //lanza el hilo de lectura de las imágenes
    facedir.wait = ini_opt("wait");

    facedir.np_to_show_imgs=0;

    cvNamedWindow(window[0], 1);
    cvNamedWindow(window[1], 1);

    facedir.filedata=0;
    facedir.lastframe=0;

    facedir.iniframe = ini_opt("iniframe");
    facedir.endframe = ini_opt("endframe");

   
    facedir.fout = NULL;
    facedir.save_video = ini_opt("ov");
    facedir.save_pose = ini_opt("op");
    facedir.videofile =0;

    return facedir.main();
}



int CFacedir::main(void)
{
    //entorno grafico  
    int i;
    std::string buffer;

    //lanza el hilo de lectura de las imágenes
    reader.init_params(ini_opts("v"), ini_opts("format"), iniframe);
    reader.set_image_size(0, 700);
    reader.start();
    
    FILE *f;
    buffer = reader.get_pathbasename()+ format(".dat.%d", iniframe);
    f=fopen(buffer.c_str(), "r");
    if (!f)
	buffer = reader.get_pathbasename()+ ".dat";
    printf("Fichero de modelo 3D: %s\n", buffer.c_str());
    npoints = ReadModelFromFile(buffer.c_str(), &points2d[0], &points2d[1], 
				&points3D, &angY, &chesscorners);
    if (npoints == 0)
    {
	printf("Error. El modelo no es correcto. 0 puntos en el modelo\n");
	exit(-1);
    }

    do { 	//hace un repaso para elegir el frame inicial
	reader.consumer();
	draw_points();
	cvShowImage(window[0], reader.img(0));
	cvShowImage(window[1], reader.img(1));
    } while (0);//bucle_cvWaitKey(&wait)>=0);
    
    frame2 = reader.head(0)->nframe;

    init();

     /************************************************************
     ** comienzo del algoritmo
     ***********************************************************/
   
    cvNamedWindow(Model_SMAT[0], 1);
    MostrarModelo();
    
    //mostrar modelos iniciales sobre las imágenes
    DemostrarModelo();

    
    cvResizeWindow(Model_SMAT[0], 600,600);
#ifdef SHOW_RIGHT
    cvNamedWindow(Model_SMAT[1], 1);
    cvResizeWindow(Model_SMAT[1], 600,600);
#endif

    Modelo(points3D, points2d[0], NULL, calibration_paramsL.KK, rotation, translationL, vectL);

    ChangeViewLR();

    init_smat();
    init2();
    init3();

    //*************************************************************


    GetNotHiden(vectL, angY, maskL);
    GetNotHiden(vectR, angY, maskR);
    float errorL, errorR;

    while(!quit)
    {
	copy_backup();
	ResizeImages();
	frame2 = reader.head(0)->nframe;

	if (!frame)
	    facedir_control();

	SMAT(&params_left);
	display_SMATModel(displayL, srcL.get(), smatL);
	inliersL = RANSAC_posit(points3D, points2d[0], maskL, maskoutL, 
				calibration_paramsL.KK, rotation, translationL, vectL, 
				&errorL); 

	SMAT(&params_right);
	display_SMATModel(displayR, srcR.get(), smatR);
	inliersR = RANSAC_posit(points3D, points2d[1], maskR, maskoutR, 
				calibration_paramsR.KK, rotation, translationR, vectR, 
				&errorR); 

	facedir();

	facedir_control();

	frame ++;

	if (!quit)
	    if (!reader.consumer())
		break;

    }

    //libera los buffers que se crearon para poder revobinar
    for (i=0; i<SHOW_IMAGE_BUFFER_SIZE; i++)
    {
	cvReleaseImage(&buffer_show_imgsL[i]);
	cvReleaseImage(&buffer_display_smatL[i]);
	cvReleaseImage(&buffer_show_imgsR[i]);
	cvReleaseImage(&buffer_display_smatR[i]);
    }
    if (fout)
	fclose(fout);

    if (videofile)
	fclose(videofile);
    return 0;
}

struct option2 mainopts2[]= {
    {{"v", 1, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "Video to process"},
    {{"m", 1, NULL, OPT_VAL_IS_CHAR}, 0, "model_test_31.xml",
     "The file containing the SMAT definition."},
    {{"format", 0, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "format = [raw | ra2 | avi]: Tells the format of the video"},
    {{"op", 0, NULL, true}, false, "", 
     "Saves the pose values after each frame. The file name will be <FILE>_pose_results.xml"},
    {{"ov", 0, NULL, true}, false, "", 
     "Saves a video of the results. They will be stored in video_results_SMAT.avi"
     " and video_results_FaceDir.avi"}, 
    {{"lines", 1, NULL, OPT_VAL_IS_INT}, 4, "", 
     "tIndicates de number of lines to draw which connects from each node"},
    {{"r", 0, NULL, true}, false, "", 
     "Read Model: Does not autocreate the Model. Reads if from a file"},
    {{"wait", 1, NULL, OPT_VAL_IS_INT}, 0, "", 
     "tIndicates de parameter for the cvWaitKey(n)"},
    {{"cam", 0, NULL, true}, false, "", 
     "Capture de video from the cameras"},

    {{"groundtruth", 0, NULL, true}, false, "", 
     "procesado del groundtruth"},
    {{"dat", 1, NULL, OPT_VAL_IS_INT}, 2, "", 
     "indica el numero N del archivo .daN a leer como datos del modelo"},
    {{"data", 1, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "..."},
    {{"graficar", 0, NULL, true}, false, "", 
     "Muestra le proceso de matching 3D"},

    {{"options", 0, NULL, true}, false, "", 
     "Imprime las opciones de configuracion del programa"},
    {{"h", 0, NULL, true}, false, "", 
     ""},
    {{"inivect", 1, NULL, OPT_VAL_IS_CHAR}, 0, "",
     "Vector inicial de direcicón"},

    {{"iniframe", 1, NULL, OPT_VAL_IS_INT}, -1, "",
     "inidica el frame de inicio"},
    {{"endframe", 1, NULL, OPT_VAL_IS_INT}, -1, "",
     "inidica el frame de fin"},
    {{"h", 0, NULL, true}, false, "", 
     "Muestra las opciones que se pueden indicar por línea de comandos"},
    {{"help", 0, NULL, true}, false, "", 
     "Muestra esta ventana de ayuda"},
    {{NULL, 0, NULL, 0}, 0, "", 
     ""}
};
